Trajectory control device

ABSTRACT

A trajectory control device includes: a contact sensor that can contact side surfaces of a workpiece; an actuator that moves a trajectory tracking member and the contact sensor; and a trajectory controller that calculates XY coordinates of a trajectory on the workpiece that is placed in an arbitrary position, by transforming XY coordinates of the trajectory on the workpiece in a reference position, based on positional information about the side surfaces of the workpiece in the reference position and positional information about the side surfaces of the workpiece placed in the arbitrary position. The positional information about the side surfaces of the workpiece placed in the arbitrary position is obtained by the contact sensor.

CROSS-REFERENCE TO RELATED APPLICATION

This application is based upon and claims the benefit of priority fromJapanese Patent Application No. 2019-211058 filed on Nov. 22, 2019, thecontents of which are incorporated herein by reference.

BACKGROUND OF THE INVENTION Field of the Invention

The present invention relates to a trajectory control device for movinga member such as a dispenser along a trajectory on a workpiece.

Description of the Related Art

Trajectory control in operations such as line application withdispensers, welding, and so on using industrial robots utilizes sensortechnologies such as high-resolution cameras, image sensors, and so on,in order to detect the position of the trajectory and correct trajectoryerror caused by positional displacement of the workpiece.

For example, Japanese Laid-Open Patent Publication No. 2008-272814describes a robot system that performs a tracking control of correctingthe position of a welding torch while detecting the position of thewelding line using a sensor capable of radiating laser slit light.

SUMMARY OF THE INVENTION

However, using a high-resolution camera or image sensor requiresintroducing an expensive and complicated system.

The present invention has been devised considering such a situation, andan object of the present invention is to provide a trajectory controldevice capable of performing trajectory correction by recognizingpositional displacement and the amount of movement of a workpiece,without using a high-precision camera or image sensor.

An aspect of the present invention is directed to a trajectory controldevice for moving a trajectory tracking member along a trajectory on aworkpiece that is placed in an arbitrary position. The trajectorycontrol device includes: a contact sensor configured to contact sidesurfaces of the workpiece; an actuator configured to move the trajectorytracking member and the contact sensor; and a trajectory controllerconfigured to calculate XY coordinates of the trajectory on theworkpiece placed in the arbitrary position, by transforming XYcoordinates of the trajectory on the workpiece in a reference position,based on positional information about the side surfaces of the workpiecein the reference position and positional information about the sidesurfaces of the workpiece placed in the arbitrary position. Thepositional information about the side surfaces of the workpiece placedin the arbitrary position is obtained by the contact sensor.

According to the trajectory control device, it is possible to obtain XYcoordinates of a trajectory on a workpiece that is placed in anarbitrary position by means of a simple method without using ahigh-precision camera or image sensor.

The trajectory control device of the invention calculates the XYcoordinates of the trajectory on the workpiece placed in an arbitraryposition from known data indicating XY coordinates of the trajectory onthe workpiece in a reference position, based on positional informationabout side surfaces of the workpiece that is obtained by the contactsensor. Accordingly, it is possible to obtain the XY coordinates of thetrajectory on the workpiece placed in the arbitrary position by means ofa simple method without using a high-precision camera or image sensor.

The above and other objects, features, and advantages of the presentinvention will become more apparent from the following description whentaken in conjunction with the accompanying drawings, in which apreferred embodiment of the present invention is shown by way ofillustrative example.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a conceptual diagram of a trajectory control device accordingto an embodiment of the present invention;

FIG. 2 is a diagram illustrating an XY coordinate system of thetrajectory control device of FIG. 1 and a workpiece placed therein;

FIG. 3 is a diagram illustrating data that is stored in a control tableT1 in a specific example in which a workpiece has moved withoutrotating;

FIG. 4 is a diagram illustrating data that is stored in a control tableT2 in the specific example of FIG. 3;

FIG. 5 is a diagram illustrating data that is stored in control tablesT3 to T5 in the specific example of FIG. 3;

FIG. 6 is a diagram illustrating data that is stored in a control tableT6 in the specific example of FIG. 3;

FIG. 7 is a diagram illustrating data that is stored in a control tableT7 in the specific example of FIG. 3;

FIG. 8 is a diagram illustrating the results of a coordinatetransformation of a workpiece alignment in the specific example of FIG.3;

FIG. 9 is a diagram illustrating the results of a coordinatetransformation of trajectory data in the specific example of FIG. 3;

FIG. 10 is a diagram illustrating data that is stored in the controltable T1 in a specific example in which a workpiece has moved withrotating;

FIG. 11 is a diagram illustrating data that is stored in the controltable T2 in the specific example of FIG. 10;

FIG. 12 is a diagram illustrating data that is stored in the controltables T3 to T5 in the specific example of FIG. 10;

FIG. 13 is a diagram illustrating data that is stored in the controltable T6 in the specific example of FIG. 10;

FIG. 14 is a diagram illustrating data that is stored in the controltable T7 in the specific example of FIG. 10;

FIG. 15 is a diagram illustrating the results of a coordinatetransformation of a workpiece alignment in the specific example of FIG.10; and

FIG. 16 is a diagram illustrating the results of a coordinatetransformation of trajectory data in the specific example of FIG. 10.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

The trajectory control device of the invention will be described belowin connection with preferred embodiments while referring to theaccompanying drawings.

As shown in FIG. 1, a trajectory control device 10 includes a trajectorycontroller 12, a contact sensor 26, a trajectory tracking member 28, anda group of electric actuators 24. The trajectory controller 12 includesa trajectory control unit 14, a motor control unit 16, anamount-of-correction operating unit 18, a coordinate transformation unit20, and an I/O unit (input/output unit) 22. Each unit of the trajectorycontroller 12 is realized by one or multiple pieces of hardware andsoftware.

The trajectory controller 12, mainly the trajectory control unit 14,performs a detection control to detect the position of a workpiece W bymoving the contact sensor 26 by driving the group of electric actuators24, and also performs a trajectory control to move the trajectorytracking member 28 along a trajectory on the workpiece by driving thegroup of electric actuators 24.

The contact sensor 26 detects the position of the workpiece W bymechanically contacting side surfaces of the workpiece W. The trajectorytracking member 28 is a member corresponding to the tip of a weldinggun, or the tip of a dispenser nozzle for supplying adhesive or sealmaterial, and is attached to a device installed on a welding line orapplication line.

The group of electric actuators 24 includes an X-axis actuator 24 a, aY-axis actuator 24 b, and a Z-axis actuator 24 c. The contact sensor 26and the trajectory tracking member 28 are both attached to the Z-axisactuator 24 c. The X-axis actuator 24 a and the Y-axis actuator 24 bserve to move the contact sensor 26 and the trajectory tracking member28 to an arbitrary position in the plane of the XY coordinate system.

The Z-axis actuator 24 c serves to adjust the positions of the contactsensor 26 and the trajectory tracking member 28 along the Z-axisdirection (up-down direction). When the trajectory controller 12performs the detection control, the contact sensor 26 moves downward toan operating position, while the trajectory tracking member 28 movesupward to a withdrawn position. On the other hand, when the trajectorycontroller 12 performs the trajectory control, the trajectory trackingmember 28 moves downward to an operating position, while the contactsensor 26 moves upward to a withdrawn position.

The trajectory control unit 14 holds trajectory control data includingdata relating to the trajectory on the workpiece and motor control data.The trajectory data is electronic data such as DXF files created by CAD,and is formed of attribute commands such as Polyline and Arc and XYcoordinate data. The motor control data is electronic data indicatingthe position, speed, acceleration, and so on of the X-axis actuator 24a, the Y-axis actuator 24 b, and the Z-axis actuator 24 c.

The trajectory control unit 14 further holds XY coordinate data thatrepresents a plane configuration of the workpiece W (which will bereferred to as “workpiece alignment” hereinafter), as well as attributedata relating to side surfaces and a reference point of the workpiece W.The workpiece alignment at least includes data relating to two sidesurfaces that are adjacent to each other with the reference pointtherebetween.

The motor control unit 16 updates and holds, in real time, encodervalues sent from motor encoders of the X-axis actuator 24 a and theY-axis actuator 24 b, as positional data of these actuators, and drivesand controls the X-axis actuator 24 a and the Y-axis actuator 24 b onthe basis of the trajectory control data from the trajectory controlunit 14.

The amount-of-correction operating unit 18 calculates the amount ofcorrection (correction coefficients) of the trajectory on the workpiece,based on positional information about the side surfaces of the workpiecedetected by the contact sensor 26. The coordinate transformation unit 20calculates coordinates of the trajectory on the workpiece that is placedin an arbitrary position, based on the amount of correction (correctioncoefficients) calculated by the amount-of-correction operating unit 18.The I/O unit 22 serves to establish synchronization with peripheraldevices, and the trajectory tracking member 28 and the contact sensor 26are connected to the I/O unit 22.

In the detection control, the trajectory controller 12 drives the X-axisactuator 24 a and the Y-axis actuator 24 b to move the contact sensor 26in a direction toward a side surface of the workpiece W. When thecontact sensor 26 comes in contact with the side surface of theworkpiece W, then the contact sensor 26 notifies the trajectorycontroller 12 through the I/O unit 22 about the contact. When thetrajectory controller 12 recognizes that the contact sensor 26 has comein contact with the side surface of the workpiece W, then the trajectorycontroller 12 stops the X-axis actuator 24 a and the Y-axis actuator 24b.

Next, the trajectory controller 12 reads the motor encoder value of theX-axis actuator 24 a and the motor encoder value of the Y-axis actuator24 b from the motor control unit 16, and holds the contact point of thecontact sensor 26 on the side surface of the workpiece W as XYcoordinate data.

This series of operations is repeated so as to obtain XY coordinate dataof different two contact points on one side surface of the workpiece W.In the same way, this series of operations is repeated also for anotherside surface of the workpiece W so as to obtain XY coordinate data ofdifferent two contact points on this side surface. The position at whichthese two side surfaces of the workpiece W intersect is defined as thereference point.

FIG. 2 is a diagram illustrating the XY coordinate system of thetrajectory control device 10 viewed from right above, and the positionof the workpiece W placed thereon, and is used to explain operations ofthe amount-of-correction operating unit 18.

In the XY coordinate system, the four side surfaces of the cuboidalworkpiece W placed in a reference position is represented by the lineconnecting a point P1 and a point P2, the line connecting the point P2and a point P3, the line connecting the point P3 and a point P4, and theline connecting the point P4 and the point P1. Among the four points P1to P4, the point P1 is the reference point. Hereinafter, the sidesurface represented by the line connecting the point P4 and the point P1is referred to as a workpiece side surface A, and the side surfacerepresented by the line connecting the point P1 and the point P2 isreferred to as a workpiece side surface B.

In this embodiment, the reference position is the position where theworkpiece W is placed in such a manner that the workpiece side surface Ais parallel to the Y axis and the workpiece side surface B is parallelto the X axis. However, for example, the reference position may be aposition where the workpiece W is placed in such a manner that theworkpiece side surface A and the workpiece side surface B are neitherparallel to the X axis nor to the Y axis.

The plane configuration of the target workpiece W need not necessarilybe rectangular, as long as the contour thereof has two straight lineparts. In other words, it works if the side surfaces of the workpiece Whave at least two flat surface portions. One or multiple pieces oftrajectory data may exist in the rectangular range of the workpiecealignment, or may exist outside of the rectangular range of theworkpiece alignment. Further, a plurality of workpieces W may be fixedto a single tray.

Now, referring to FIG. 2, the detection control will be described morespecifically. Because a workpiece alignment WP (P1-P2-P3-P4) placed inthe reference position is previously held in the trajectory controller12, it is not necessary to detect the position of the side surfaces ofthe workpiece. However, here, the description will be given assumingthat the workpiece W is first placed in the reference position.Accordingly, in practice, the detection of the side surfaces of theworkpiece may be omitted for the workpiece W placed in the referenceposition.

The contact sensor 26 advances in a first movement direction 30 that isparallel to the X axis, stops when it reaches a point A1 on theworkpiece side surface A, and detects and holds the X coordinate and theY coordinate of the point A1. In the same way, the contact sensor 26advances in a second movement direction 32 that is parallel to the Xaxis, stops when it reaches a point A2 on the workpiece side surface A,and detects and holds the X coordinate and the Y coordinate of the pointA2. In this embodiment, the first movement direction 30 is a directionalong the straight line that is expressed as y=40, and the secondmovement direction 32 is a direction along the straight line that isexpressed as y=30.

Further, the contact sensor 26 advances in a third movement direction 34that is parallel to the Y axis, stops when it reaches a point B1 on theworkpiece side surface B, and detects and holds the X coordinate and theY coordinate of the point B1. In the same way, the contact sensor 26advances in a fourth movement direction 36 that is parallel to the Yaxis, stops when it reaches a point B2 on the workpiece side surface B,and detects and holds the X coordinate and the Y coordinate of the pointB2. In this embodiment, the third movement direction 34 is a directionalong the straight line that is expressed as x=50, and the fourthmovement direction 36 is a direction along the straight line that isexpressed as x=40.

In this embodiment, the equation of the straight line passing through A1and A2 is given as x=10, and the equation of the straight line passingthrough B1 and B2 is given as y=10. The intersection of these twostraight lines is the reference point P1, and the XY coordinates of thereference point P1 are (10, 10). As to these two straight lines, if aline passing through two points is expressed by a general expressiony=ax+b, then a coefficient “a” and a coefficient “b” are both treated aszero (a=0, b=0).

Next, a workpiece alignment WQ that is placed in an arbitrary positionwill be described. The workpiece alignment WQ placed in an arbitraryposition includes points Q1 to Q4 that correspond respectively to thefour points P1 to P4 of the workpiece alignment WP placed in thereference position. Hereinafter, the side surface represented by theline connecting the point Q4 and the point Q1 is referred to as aworkpiece side surface C, and the side surface represented by the lineconnecting the point Q1 and the point Q2 is referred to as a workpieceside surface D.

The contact sensor 26 advances in the aforementioned first movementdirection 30 (y=40) that is parallel to the X axis, stops when itreaches a point C1 on the workpiece side surface C corresponding to theworkpiece side surface A in the reference position, and detects andholds the X coordinate and the Y coordinate of the point C1. In the sameway, the contact sensor 26 advances in the aforementioned secondmovement direction 32 (y=30) that is parallel to the X axis, stops whenit reaches a point C2 on the workpiece side surface C, and detects andholds the X coordinate and the Y coordinate of the point C2.

Further, the contact sensor 26 advances in the aforementioned thirdmovement direction 34 (x=50) that is parallel to the Y axis, stops whenit reaches a point D1 on the workpiece side surface D corresponding tothe workpiece side surface B in the reference position, and detects andholds the X coordinate and the Y coordinate of the point D1. In the sameway, the contact sensor 26 advances in the aforementioned fourthmovement direction 36 (x=40) that is parallel to the Y axis, stops whenit reaches a point D2 on the workpiece side surface D, and detects andholds the X coordinate and the Y coordinate of the point D2.

The equation of the straight line passing through C1 and C2 is given asy=a₁x+b₁, and the equation of the straight line passing through D1 andD2 is given as y=a₂x+b₂ (a₁≠a₂). The intersection of these two straightlines is a point Q1 to which the reference point P1 has moved. That is,the reference point P1 moves to the point Q1 when the workpiece W placedin the reference position moves to the arbitrary position. The XYcoordinates (x1, y1) of the point Q1 can be calculated by the equationsbelow.x1=(b ₂ −b ₁)/(a ₁ −a ₂)y1=(a ₁ b ₂ −a ₂ b ₁)/(a ₁ −a ₂)

If the XY coordinates of the reference point P1 are expressed as (x0,y0) and the amount of movement from the reference point P1 to the pointQ1 (x1, y1) is expressed as an amount of movement Δx of the X coordinateand an amount of movement Δy of the Y coordinate, then Δx=x1−x0 andΔy=y1−y0. In this embodiment, x0=10 and y0=10, and therefore Δx=x1−10and Δy=y1−10.

If the straight line representing the workpiece side surface B placed inthe reference position is given as y=a₀+b₀, then a gradient angle Δθthat is formed by the workpiece side surface D after movement and theworkpiece side surface B in the reference position can be calculated bythe equation below, based on this straight line and the straight liney=a₂x+b₂ representing the workpiece side surface D placed in thearbitrary position.Δθ=tan⁻¹((a ₂ −a ₀)/(1+a ₀ a ₂))

In this embodiment, a₀=0 and therefore Δθ=tan⁻¹a₂.

The trajectory data after movement can be obtained by calculation fromthe trajectory data in the reference position if the amount of movementΔx of the X coordinate of the reference point P1, the amount of movementΔy of the Y coordinate of the reference point P1, and the gradient angleΔθ have been detected.

Referring to FIGS. 3 to 16, a specific method for the trajectory controlwill be described separately in a case where the workpiece W has movedfrom the reference position without rotating (positional displacement)and a case where the workpiece W has moved from the reference positionwith rotating (positional displacement).

[Specific Example without Rotation]

First, referring to FIGS. 3 to 9, a specific example will be describedin which the workpiece W has moved from a reference position withoutrotating. It is assumed that the reference position of the workpiece Wis the same as that shown in FIG. 2.

As shown in FIG. 3, the control table T1 stores XY coordinate data aboutthe four points A1, A2, B1 and B2 on the workpiece alignment WP in thereference position, and XY coordinate data about the four points C1, C2,D1 and D2 on the workpiece alignment WQ after having moved.

Based on the XY coordinate data in the control table T1, the equation ofthe straight line passing through A1 and A2, the equation of thestraight line passing through B1 and B2, the equation of the straightline passing through C1 and C2, and the equation of the straight linepassing through D1 and D2, are obtained and stored in a control table T2as shown in FIG. 4. These four straight lines correspond to theworkpiece side surface A, the workpiece side surface B, the workpieceside surface C, and the workpiece side surface D, respectively.

In this specific example, the straight line passing through A1 and A2 (Astraight line) and the straight line passing through C1 and C2 (Cstraight line) are both parallel to the Y axis, and the straight linepassing through B1 and B2 (B straight line) and the straight linepassing through D1 and D2 (D straight line) are both parallel to the Xaxis. The workpiece side surface A (A surface) is stored as x=10.00, theworkpiece side surface B (B surface) is stored as y=10.00, the workpieceside surface C (C surface) is stored as x=15.00, and the workpiece sidesurface D (D surface) is stored as y=17.00. Further, as to these fourstraight lines, the coefficient “a” and the coefficient “b” in thegeneral expression of straight line y=ax+b are all stored as zero(0.00).

Based on the equations of the straight lines in the control table T2,the XY coordinates of the intersection of the workpiece side surface Aand the workpiece side surface B, and the XY coordinates of theintersection of the workpiece side surface C and the workpiece sidesurface D, are obtained and stored in a control table T3 as shown inFIG. 5. The data stored in the control table T3 indicates that the Xcoordinate and Y coordinate of the intersection of the workpiece sidesurface A and the workpiece side surface B, i.e., the reference point P1(x0, y0), are both 10.00. Also, the data indicates that the X coordinateand Y coordinate of the intersection of the workpiece side surface C andthe workpiece side surface D, i.e., the point Q1 (x1, y1) to which thereference point P1 has moved, are 15.00 and 17.00, respectively.

The amount of movement Δx of the X coordinate and the amount of movementΔy of the Y coordinate from the reference point P1 (x0, y0) to the pointQ1 (x1, y1) are given as Δx=x1−x0=5.00 and Δy=y1−y0=7.00, respectively.As shown in FIG. 5, the value of the amount of movement Δx of the Xcoordinate is stored in a control table T4 as an X-coordinate amount ofmovement (work_tx) of the workpiece W, and the value of the amount ofmovement Δy of the Y coordinate is stored in the control table T4 as aY-coordinate amount of movement (work_ty) of the workpiece W.

Further, if the straight line that represents the workpiece side surfaceB in the reference position is given as y=a₀x+b₀ and the straight linethat represents the workpiece side surface D after movement is given asy=a₂x+b₂, then, as stated earlier, the gradient angle Δθ of theworkpiece side surface D with respect to the workpiece side surface Bcan be calculated by the equation below.Δθ=tan⁻¹((a ₂ −a ₀)/(1+a ₀ a ₂))

In this specific example, a₀=0 and a₂=0, and therefore Δθ=0, which is,as shown in FIG. 5, stored in a control table T5 as 0.00 radians and0.00 degrees.

If the amount of movement Δx of the X coordinate of the reference pointP1, the amount of movement Δy of the Y coordinate of the reference pointP1, and the gradient angle Δθ of the workpiece side surface aftermovement with respect to the workpiece side surface in the referenceposition are known, then the XY coordinate data of the trajectory afterhaving moved can be obtained by calculation from the XY coordinate dataof the trajectory at the time when the workpiece W is in the referenceposition. As mentioned earlier, the XY coordinate data of the trajectoryat the time when the workpiece W is in the reference position are heldin the trajectory control unit 14.

Specifically, the XY coordinates (x′, y′) of the trajectory aftermovement can be calculated by the following equations by performing acoordinate transformation called affine transformation on the XYcoordinates (x, y) of the trajectory in the reference position.x′=(p)(x)Cos(Δθ)−(q)(y)Sin(Δθ)+txy′=(p)(x)Sin(Δθ)+(q)(y)Cos(Δθ)+ty

Now, p=1 and q=1, assuming that the workpiece size is not enlarged orreduced. Further, since the coordinate transformation is performedthrough rotation about the origin (0, 0) of the XY coordinate system,first, the amount of movement tx in the X direction and the amount ofmovement ty in the Y direction are set at zero in the affinetransformation expressions (tx=0, ty=0), and then an amount of movementaff_tx of the X coordinate and an amount of movement aff_ty of the Ycoordinate of the reference point by the affine transformation arecalculated.

In this specific example, Δθ=0. Accordingly, x=10, y=10 and Δθ=0 aresubstituted into the following expressions, and then the X coordinateand Y coordinate of the reference point P1 (10, 10) after movement bythe affine transformation are both calculated to be 10.X coordinate after movement=(x)Cos(Δθ)−(y)Sin(Δθ)Y coordinate after movement=(x)Sin(Δθ)+(y)Cos(Δθ)

The amount of movement aff_tx of the X coordinate and the amount ofmovement aff_ty of the Y coordinate of the reference point by the affinetransformation are obtained as follows (the XY coordinates beforemovement are the XY coordinates of the reference point P1).aff_tx=X coordinate before movement−X coordinate after movement=10−10=0aff_ty=Y coordinate before movement−Y coordinate after movement=10−10=0As shown in FIG. 6, these values are stored in a control table T6 as0.00.

Thus, a rotation angle θ=0.00 (degrees),X-direction amount of movement tx=aff_tx+work_tx=0.00+5.00=5.00Y-direction amount of movement ty=aff_ty+work_ty=0.00+7.00=7.00In this way, the affine transformation (coordinate transformation)conditions for calculating the trajectory data after movement areobtained and stored in a control table T7 as shown in FIG. 7.

FIG. 8 shows the results of the coordinate transformation of theworkpiece alignments. In FIG. 8, the dotted line shows the workpiecealignment WP in the reference position and the solid line shows theworkpiece alignment WQ after movement.

Further, FIG. 9 shows the results of the coordinate transformation ofthe trajectory data. In FIG. 9, the dotted line shows the trajectory inthe reference position and the solid line shows the trajectory aftermovement.

[Specific Example with Rotation]

Next, referring to FIGS. 10 to 16, a specific example will be describedin which the workpiece W has moved from a reference position withrotating. It is assumed that the reference position of the workpiece Wis the same as that shown in FIG. 2.

As shown in FIG. 10, the control table T1 stores the XY coordinate dataabout the four points A1, A2, B1 and B2 on the workpiece alignment WP inthe reference position, and the XY coordinate data about the four pointsC1, C2, D1 and D2 on the workpiece alignment WQ after having moved.

Based on the XY coordinate data in the control table T1, the equation ofthe straight line passing through A1 and A2, the equation of thestraight line passing through B1 and B2, the equation of the straightline passing through C1 and C2, and the equation of the straight linepassing through D1 and D2, are obtained and stored in the control tableT2 as shown in FIG. 11. These four straight lines correspond to theworkpiece side surface A, the workpiece side surface B, the workpieceside surface C, and the workpiece side surface D, respectively.

In this specific example, the straight line passing through A1 and A2 (Astraight line) is parallel to the Y axis, and the straight line passingthrough B1 and B2 (B straight line) is parallel to the X axis. Theworkpiece side surface A (A surface) is stored as x=10.00 and theworkpiece side surface B (B surface) is stored as y=10.00. Further, asto these two straight lines, the coefficient “a” and the coefficient “b”in the general expression of straight line y=ax+b are all stored as zero(0.00).

On the other hand, the straight line passing through C1 and C2 (Cstraight line) and the straight line passing through D1 and D2 (Dstraight line) are neither parallel to the X axis nor to the Y axis. Theworkpiece side surface C (C surface) is given as y=−3.33x+140, and thevalue of the coefficient “a” and the value of the coefficient “b” in thegeneral expression of straight line y=ax+b are stored as −3.33 and140.00, respectively. Further, the workpiece side surface D (D surface)is given as y=0.3x+8 and the value of the coefficient “a” is stored as0.30 and the value of the coefficient “b” is stored as 8.00.

Based on the equations of the straight lines in the control table T2,the XY coordinates of the intersection of the workpiece side surface Aand the workpiece side surface B, and the XY coordinates of theintersection of the workpiece side surface C and the workpiece sidesurface D, are obtained and stored in the control table T3 as shown inFIG. 12. The data stored in the control table T3 indicates that the Xcoordinate and Y coordinate of the intersection of the workpiece sidesurface A and the workpiece side surface B, i.e., the reference point P1(x0, y0), are both 10.00. Also, the data indicates that the X coordinateand Y coordinate of the intersection of the workpiece side surface C andthe workpiece side surface D, i.e., the point Q1 (x1, y1) to which thereference point P1 has moved, are 36.33 and 18.90, respectively.

The amount of movement Δx of the X coordinate and the amount of movementΔy of the Y coordinate from the reference point P1 (x0, y0) to the pointQ1 (x1, y1) are given as Δx=x1−x0=26.33 and Δy=y1−y0=8.90, respectively.As shown in FIG. 12, the value of the amount of movement Δx of the Xcoordinate is stored as work_tx in the control table T4, and the valueof the amount of movement Δy of the Y coordinate is stored as work_ty inthe control table T4.

Further, if the straight line that represents the workpiece side surfaceB in the reference position is given as y=a₀x+b₀ and the straight linethat represents the workpiece side surface D after movement is given asy=a₂x+b₂, then, as stated earlier, the gradient angle Δθ of theworkpiece side surface D with respect to the workpiece side surface Bcan be calculated by the equation below.Δθ=tan⁻¹((a ₂ −a ₀)/(1+a ₀ a ₂))

In this specific example, a₀=0 and a₂=0.30, and therefore Δθ=0.29, whichis, as shown in FIG. 12, stored in the control table T5 as 0.29 radiansand 16.70 degrees.

As has been explained above, the XY coordinates (x′, y′) of thetrajectory after movement can be calculated by the following equationsby performing the affine transformation on the XY coordinates (x, y) ofthe trajectory in the reference position.x′=(p)(x)Cos(Δθ)−(q)(y)Sin(Δθ)+txy′=(p)(x)Sin(Δθ)+(q)(y)Cos(Δθ)+ty

Now, p=1 and q=1, assuming that the workpiece size is not enlarged orreduced. Further, since the coordinate transformation is performedthrough rotation about the origin (0, 0) of the XY coordinate system,the amount of movement tx in the X direction and the amount of movementty in the Y direction are set at zero in the affine transformationexpressions (tx=0, ty=0), and then the amount of movement aff_tx of theX coordinate and the amount of movement aff_ty of the Y coordinate ofthe reference point by the affine transformation are calculated.

In this specific example, Δθ=0.29 radians. Accordingly, x=10, y=10 andΔθ=0.29 are substituted into the following expressions, and then the Xcoordinate and Y coordinate of the reference point P1 (10, 10) aftermovement by the affine transformation are calculated to be 6.70 and12.45, respectively.X coordinate after movement=(x)Cos(Δθ)−(y)Sin(Δθ)Y coordinate after movement=(x)Sin(Δθ)+(y)Cos(Δθ)

The amount of movement aff_tx of the X coordinate and the amount ofmovement aff_ty of the Y coordinate of the reference point by the affinetransformation are obtained as follows (the XY coordinates beforemovement are the XY coordinates of the reference point P1).aff_tx=X coordinate before movement−X coordinate aftermovement=10−6.70=3.30aff_ty=Y coordinate before movement−Y coordinate aftermovement=10−12.45=−2.45As shown in FIG. 13, these values are stored in the control table T6.

Seen from a different perspective, the amount of movement of thereference point by the affine transformation is for adjustment of thedifference between the XY coordinates after rotation in the case where acoordinate point (x, y) in the XY coordinate system is rotated by Δθabout the origin of the XY coordinate system, and those in the casewhere it is rotated by Δθ about the reference point, and the amount ofmovement of the reference point by the affine transformation takesconstant values corresponding to the value of Δθ and the values of theXY coordinates of the reference point, irrespective of the position ofthe coordinate point (x, y). Then, the amount of movement aff_tx of theX coordinate of the reference point by the affine transformation can beobtained by calculating the difference between the X coordinate of thereference point P1 and the X coordinate of the reference point P1rotated by Δθ about the origin of the XY coordinate system. In the sameway, the amount of movement aff_ty of the Y coordinate of the referencepoint by the affine transformation can be obtained by calculating thedifference between the Y coordinate of the reference point P1 and the Ycoordinate of the reference point P1 rotated by Δθ about the origin ofthe XY coordinate system.

Thus, the rotation angle θ=16.70 (degrees),X-direction amount of movement tx=aff_tx+work_tx=3.30+26.33=29.63Y-direction amount of movement ty=aff_ty+work_ty=−2.45+8.90=6.45In this way, the affine transformation (coordinate transformation)conditions for calculating the trajectory data after movement areobtained and stored in the control table T7 as shown in FIG. 14.

FIG. 15 shows the results of the coordinate transformation of theworkpiece alignments. In FIG. 15, the dotted line shows the workpiecealignment WP in the reference position and the solid line shows theworkpiece alignment WQ after movement.

Further, FIG. 16 shows the results of the coordinate transformation ofthe trajectory data. In FIG. 16, the dotted line shows the trajectory inthe reference position and the solid line shows the trajectory aftermovement.

Thus, the trajectory control device 10 of the embodiment can obtain theamount of movement Δx of the X coordinate of the reference point P1, theamount of movement Δy of the Y coordinate of the reference point P1, andthe gradient angle Δθ of a workpiece side surface after movement withrespect to the workpiece side surface in the reference position, bymeans of a simple detection control using the contact sensor 26, withoutusing a high-precision camera or image sensor, and can correct thetrajectory in real time on the basis of these pieces of data.

More specifically, if the XY coordinates of two contact points of thecontact sensor 26 on the workpiece side surface C and the XY coordinatesof two contact points of the contact sensor 26 on the workpiece sidesurface D are known, then the amount of movement Δx of the X coordinate,the amount of movement Δy of the Y coordinate, and the gradient angle Δθcan be obtained, and then the XY coordinates of the trajectory on theworkpiece placed in an arbitrary position can be easily calculated byusing the affine transformation.

Further, the contact sensor 26 and the trajectory tracking member 28 areboth attached to the Z-axis actuator 24 c, and therefore the contactsensor 26 and the trajectory tracking member 28 can be driven by thesame mechanism.

Furthermore, the X-axis actuator 24 a and the Y-axis actuator 24 b arestopped when the contact sensor 26 comes in contact with the workpieceside surface C or workpiece side surface D, which prevents the workpieceW placed in the arbitrary position from moving during the detectioncontrol.

The trajectory control device of the present invention is not limited tothe embodiments described above, but can of course adopt variousconfigurations without departing from the essence and gist of thepresent invention.

What is claimed is:
 1. A trajectory control device for moving atrajectory tracking member along a trajectory on a workpiece that isplaced in an arbitrary position, the trajectory control devicecomprising: a contact sensor configured to contact side surfaces of theworkpiece; an actuator configured to move the trajectory tracking memberand the contact sensor; and a trajectory controller configured tocalculate XY coordinates of the trajectory on the workpiece placed inthe arbitrary position, by transforming XY coordinates of the trajectoryon the workpiece in a reference position, based on positionalinformation about the side surfaces of the workpiece in the referenceposition and positional information about the side surfaces of theworkpiece placed in the arbitrary position, wherein the positionalinformation about the side surfaces of the workpiece placed in thearbitrary position is obtained by the contact sensor, wherein thetrajectory controller detects XY coordinates of two contact points onone side surface of the workpiece by driving the actuator to advance thecontact sensor in two movement directions that are parallel to an Xaxis, and detects XY coordinates of two contact points on another sidesurface of the workpiece by driving the actuator to advance the contactsensor in two movement directions that are parallel to a Y axis, andwherein the trajectory controller obtains an equation of a firststraight line that passes through the two contact points on the one sidesurface of the workpiece and an equation of a second straight line thatpasses through the two contact points on the another side surface of theworkpiece, further obtains XY coordinates of an intersection of thefirst straight line and the second straight line, then calculates anamount of movement of an X coordinate and an amount of movement of a Ycoordinate of a reference point of the workpiece, and also calculates agradient angle of the another side surface of the workpiece with respectto the reference position of the workpiece.
 2. The trajectory controldevice according to claim 1, wherein the actuator includes an X-axisactuator, a Y-axis actuator, and a Z-axis actuator, and the contactsensor and the trajectory tracking member are attached to the Z-axisactuator.
 3. The trajectory control device according to claim 2, whereinpositions of the contact sensor and the trajectory tracking member in aZ-axis direction are adjusted by driving the Z-axis actuator in such amanner that the trajectory tracking member moves to a withdrawn positionwhen the contact sensor moves to an operating position, and the contactsensor moves to a withdrawn position when the trajectory tracking membermoves to an operating position.
 4. The trajectory control deviceaccording to claim 1, wherein the trajectory controller stops theactuator when the contact sensor comes in contact with the one sidesurface or the another side surface.
 5. The trajectory control deviceaccording to claim 1, wherein the XY coordinates of the trajectory onthe workpiece placed in the arbitrary position are calculated by usingan affine transformation, based on the amount of movement of the Xcoordinate of the reference point of the workpiece, the amount ofmovement of the Y coordinate of the reference point of the workpiece,and the gradient angle.
 6. The trajectory control device according toclaim 5, wherein the affine transformation is performed by followingequations (meanings of letters and signs in the equations are shownbelow):x′=(x)Cos(Δθ)−(y)Sin(Δθ)+txy′=(x)Sin(Δθ)+(y)Cos(Δθ)+ty where x′: the X coordinate of the trajectoryon the workpiece placed in the arbitrary position, y′: the Y coordinateof the trajectory on the workpiece placed in the arbitrary position, x:the X coordinate of the trajectory on the workpiece in the referenceposition, y: the Y coordinate of the trajectory on the workpiece in thereference position, Δθ: the gradient angle, tx: the amount of movementof the X coordinate of the reference point of the workpiece plus theamount of movement of the X coordinate of the reference point by theaffine transformation, and ty: the amount of movement of the Ycoordinate of the reference point of the workpiece plus the amount ofmovement of the Y coordinate of the reference point by the affinetransformation.
 7. The trajectory control device according to claim 6,wherein the amount of movement of the X coordinate of the referencepoint by the affine transformation is a calculated difference betweenthe X coordinate of the reference point and the X coordinate of thereference point rotated by the gradient angle Δθ about an origin of anXY coordinate system, and the amount of movement of the Y coordinate ofthe reference point by the affine transformation is a calculateddifference between the Y coordinate of the reference point and the Ycoordinate of the reference point rotated by the gradient angle Δθ aboutthe origin of the XY coordinate system.